# -----------------------------------------------------------------------------
# This file was autogenerated by symforce from template:
#     backends/python/templates/function/FUNCTION.py.jinja
# Do NOT modify by hand.
# -----------------------------------------------------------------------------

import math  # pylint: disable=unused-import
import numpy  # pylint: disable=unused-import
import typing as T  # pylint: disable=unused-import

import sym  # pylint: disable=unused-import


# pylint: disable=too-many-locals,too-many-lines,too-many-statements,unused-argument


def fuse_airspeed(v_local, state, P, airspeed, R, epsilon):
    # type: (T.Sequence[float], T.Sequence[float], numpy.ndarray, float, float, float) -> T.Tuple[numpy.ndarray, T.List[float], float, float]
    """
    This function was autogenerated from a symbolic function. Do not modify by hand.

    Symbolic function: fuse_airspeed

    Args:
        v_local: Matrix31
        state: Matrix31
        P: Matrix33
        airspeed: Scalar
        R: Scalar
        epsilon: Scalar

    Outputs:
        H: Matrix13
        K: Matrix31
        innov_var: Scalar
        innov: Scalar
    """

    # Total ops: 56

    # Input arrays

    # Intermediate terms (11)
    _tmp0 = -state[0] + v_local[0]
    _tmp1 = -state[1] + v_local[1]
    _tmp2 = math.sqrt(_tmp0 ** 2 + _tmp1 ** 2 + epsilon + v_local[2] ** 2)
    _tmp3 = state[2] / _tmp2
    _tmp4 = _tmp0 * _tmp3
    _tmp5 = _tmp1 * _tmp3
    _tmp6 = -P[0] * _tmp4
    _tmp7 = -P[4] * _tmp5
    _tmp8 = P[8] * _tmp2
    _tmp9 = (
        R
        + _tmp2 * (-P[6] * _tmp4 - P[7] * _tmp5 + _tmp8)
        - _tmp4 * (-P[1] * _tmp5 + P[2] * _tmp2 + _tmp6)
        - _tmp5 * (-P[3] * _tmp4 + P[5] * _tmp2 + _tmp7)
    )
    _tmp10 = max(_tmp9, epsilon) ** (-1)

    # Output terms
    _H = numpy.zeros((1, 3))
    _H[0, 0] = -_tmp4
    _H[0, 1] = -_tmp5
    _H[0, 2] = _tmp2
    _K = [0.0] * 3
    _K[0] = _tmp10 * (-P[3] * _tmp5 + P[6] * _tmp2 + _tmp6)
    _K[1] = _tmp10 * (-P[1] * _tmp4 + P[7] * _tmp2 + _tmp7)
    _K[2] = _tmp10 * (-P[2] * _tmp4 - P[5] * _tmp5 + _tmp8)
    _innov_var = _tmp9
    _innov = -_tmp2 * state[2] + airspeed
    return _H, _K, _innov_var, _innov
